Fix for multiple video plugins (#898) #937

shiveshkhaitan · 2019-06-19T11:57:24Z

Append the name of the plugin specified in the sdf to avoid crashing when multiple video plugins are used. This connects to #898

chapulina · 2019-06-26T16:41:43Z

With this PR, I'm able to add another display to the demo world like this, and both displays work:

diff --git a/gazebo_plugins/worlds/gazebo_ros_video_demo.world b/gazebo_plugins/worlds/gazebo_ros_video_$emo.world
index 46766d2..74e05b3 100644
--- a/gazebo_plugins/worlds/gazebo_ros_video_demo.world
+++ b/gazebo_plugins/worlds/gazebo_ros_video_demo.world
@@ -159,5 +159,34 @@
       </link>
     </model>

+    <model name="box_display_2">
+      <pose>3.0 0.0 0.0 0.0 0.0 0.0</pose>
+      <link name="base_link">
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.5 0.5 0.5</size>
+            </box>
+          </geometry>
+          <plugin name="display_video_controller_2" filename="libgazebo_ros_video.so">
+            <ros>
+              <!-- Remap for image topic to be subscribed -->
+              <argument>~/image_raw:=/camera1/image_raw</argument>
+            </ros>
+            <height>120</height>
+            <width>160</width>
+          </plugin>
+        </visual>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.5 0.5 0.5</size>
+            </box>
+          </geometry>
+          <laser_retro>100.0</laser_retro>
+        </collision>
+      </link>
+    </model>
+
   </world>

But Gazebo seems to hang on shutdown and needs to be force-closed. Can you reproduce the issue? It would be good to look into it.

shiveshkhaitan · 2019-06-27T07:46:14Z

Fixed the crashing of gazebo on shutdown

chapulina · 2019-07-09T12:27:44Z

gazebo_plugins/src/gazebo_ros_video.cpp

@@ -186,6 +186,9 @@ GazeboRosVideo::GazeboRosVideo()
 // Destructor
 GazeboRosVideo::~GazeboRosVideo()
 {
+  if (rclcpp::is_initialized()) {
+    rclcpp::shutdown();
+  }


This solution is a bit too drastic, because we still want all other plugins to keep working after one plugin is destroyed. You can try for example adding the 2nd video box as on my other comment and then deleting one of the boxes. This line will be called and the other box's video will stop.

Is there another way of fixing the shutdown issue?

For me the other box's video is stopping even without rclcpp::shutdown(). Also I show that this

gazebo_ros_pkgs/gazebo_ros/include/gazebo_ros/node.hpp

Lines 139 to 142 in 1200972

if (!rclcpp::is_initialized()) {

rclcpp::init(0, nullptr);

RCLCPP_INFO(internal_logger(), "ROS was initialized without arguments.");

}

goes into if multiple times when we have multiple video plugins. Is is supposed to happen?

Not only multiple video plugins, even if I have just one video plugin and a diff drive plugin, diff drive stops working on deleting the box

goes into if multiple times when we have multiple video plugins. Is is supposed to happen?

Ah that got me digging for a while. What's happening is that the video plugins are being loaded twice, once in gzclient, for the GUI, and once in gzserver, for the camera sensor. That happens for all VisualPlugins.

If you start only gzserver gazebo_ros_video_demo.world, you can see that there are only 2 subscribers to camera images:

$ ros2 topic info /camera1/image_raw Topic: /camera1/image_raw Publisher count: 1 Subscriber count: 2

So I think the root of the shutdown problems we're having come from these duplicate nodes being created on both client and server. There are a few ways to tackle this. I think we should load that plugin only on the server scene and not on the client. I can't remember if we have an easy way to tell if we're on the client or server from the plugin though. Since this PR the Scene keeps a member variable, but that's not exposed. Maybe @iche033 remembers some nice trick to check it?

Actually come to think of it, we probably want the plugin on both client and server... So maybe we need to give the nodes unique names.

if you ever need it, Scene::EnableVisualizations returns true if it's the client and false if it's the server

Setting different names for gzserver and gzclient did not help. But I think I figured out the problem. Adding this line executor_->remove_node(get_node_base_interface()); in gazebo_ros::Node destructor

gazebo_ros_pkgs/gazebo_ros/src/node.cpp

Lines 29 to 31 in 1200972

Node::~Node()

{

}

fixes it. What I think is that when the plugin is destroyed and the node is deleted, the executor does not remove it and tries to spin but gets stuck as the node is already deleted. The above line explicitly removes the node. Does it make sense @chapulina?

Argh it sounds like #860 is back to haunt us.

If I remember correctly, when debugging that with @wjwwood , the underlying issue seemed to be coming from the RMW layer. I don't remember if there was any problem adding the remove_node call, I think it just didn't fix the issue completely. So if it solves this specific issue without breaking anything else, I think we should get it in. Let's give it a try!

Isn't this essentially ros2/rclcpp#272? I believe it should have been fixed with ros2/rclcpp#741.

If not, then we should make sure there is an open issue on ros2/rclcpp which tracks the bug described in #860.

Unless I misunderstood and it's actually a bug in how rclcpp is being used here, I can't remember :x.

@wjwwood , I just tried uncommenting the test that was failing:

gazebo_ros_pkgs/gazebo_ros/test/test_node.cpp

Lines 71 to 108 in 5e4a989

// TODO(anyone) Fix this test, see

// https://github.com/ros-simulation/gazebo_ros_pkgs/issues/855

// Create another node

// auto sdf_str_2 =

// "<?xml version='1.0' ?>"

// "<sdf version='1.6'>"

// "<world name='default'>"

// "<plugin name='node_2' filename='libnode_name.so'/>"

// "</world>"

// "</sdf>";

// sdf::SDF sdf_2;

// sdf_2.SetFromString(sdf_str_2);

// auto plugin_sdf_2 = sdf_2.Root()->GetElement("world")->GetElement("plugin");

// auto node_2 = gazebo_ros::Node::Get(plugin_sdf_2);

// ASSERT_NE(nullptr, node_2);

// EXPECT_STREQ("node_2", node_2->get_name());

// EXPECT_NE(node_1, node_2);

// Reset both

// node_1.reset();

// node_2.reset();

// // Create another node

// auto sdf_str_3 =

// "<?xml version='1.0' ?>"

// "<sdf version='1.6'>"

// "<world name='default'>"

// "<plugin name='node_3' filename='libnode_name.so'/>"

// "</world>"

// "</sdf>";

// sdf::SDF sdf_3;

// sdf_3.SetFromString(sdf_str_3);

// auto plugin_sdf_3 = sdf_3.Root()->GetElement("world")->GetElement("plugin");

// auto node_3 = gazebo_ros::Node::Get(plugin_sdf_3);

// ASSERT_NE(nullptr, node_3);

// EXPECT_STREQ("node_3", node_3->get_name());

And the situation is still the same:

Without the remove_node call, the test fails every time

With the remove_node call, the test never passes 10 times in a row

Unfortunately, I don't have time right now to create a failing test case using just rclcpp 😕 If I remember correctly, we tried doing that together and it was complicated to reproduce what's happening here.

I'll merge the remove_node fix because it solves the issue for the video plugin, but I'll leave #855 open.

* Fix for multiple video plugins (#898) * Fix crash on shutdown * Fix gazebo node destructor

Fix for multiple video plugins (ros-simulation#898)

149c844

chapulina added the ros2 label Jun 19, 2019

chapulina self-requested a review June 19, 2019 12:28

Fix crash on shutdown

7568987

chapulina reviewed Jul 9, 2019

View reviewed changes

Fix gazebo node destructor

8f6561d

chapulina approved these changes Jul 11, 2019

View reviewed changes

chapulina merged commit 8d3f2ff into ros-simulation:ros2 Jul 11, 2019

shiveshkhaitan deleted the video_plugin branch July 11, 2019 13:18

chapulina mentioned this pull request Jul 16, 2019

[ros2] Fix tests on Dashing #953

Merged

chapulina pushed a commit that referenced this pull request Aug 11, 2019

Fix for multiple video plugins (#898) (#937)

07ea8b9

* Fix for multiple video plugins (#898) * Fix crash on shutdown * Fix gazebo node destructor

chapulina mentioned this pull request Aug 11, 2019

[backport] ros2 -> dashing #977

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix for multiple video plugins (#898) #937

Fix for multiple video plugins (#898) #937

shiveshkhaitan commented Jun 19, 2019

chapulina commented Jun 26, 2019

shiveshkhaitan commented Jun 27, 2019

chapulina Jul 9, 2019

shiveshkhaitan Jul 9, 2019

shiveshkhaitan Jul 9, 2019 •

edited

Loading

chapulina Jul 9, 2019 •

edited

Loading

chapulina Jul 9, 2019

iche033 Jul 9, 2019

shiveshkhaitan Jul 10, 2019 •

edited

Loading

chapulina Jul 10, 2019

wjwwood Jul 10, 2019

chapulina Jul 11, 2019 •

edited

Loading

	if (!rclcpp::is_initialized()) {
	rclcpp::init(0, nullptr);
	RCLCPP_INFO(internal_logger(), "ROS was initialized without arguments.");
	}

	// TODO(anyone) Fix this test, see
	// https://github.com/ros-simulation/gazebo_ros_pkgs/issues/855

	// Create another node
	// auto sdf_str_2 =
	// "<?xml version='1.0' ?>"
	// "<sdf version='1.6'>"
	// "<world name='default'>"
	// "<plugin name='node_2' filename='libnode_name.so'/>"
	// "</world>"
	// "</sdf>";
	// sdf::SDF sdf_2;
	// sdf_2.SetFromString(sdf_str_2);
	// auto plugin_sdf_2 = sdf_2.Root()->GetElement("world")->GetElement("plugin");

	// auto node_2 = gazebo_ros::Node::Get(plugin_sdf_2);
	// ASSERT_NE(nullptr, node_2);
	// EXPECT_STREQ("node_2", node_2->get_name());
	// EXPECT_NE(node_1, node_2);

	// Reset both
	// node_1.reset();
	// node_2.reset();

	// // Create another node
	// auto sdf_str_3 =
	// "<?xml version='1.0' ?>"
	// "<sdf version='1.6'>"
	// "<world name='default'>"
	// "<plugin name='node_3' filename='libnode_name.so'/>"
	// "</world>"
	// "</sdf>";
	// sdf::SDF sdf_3;
	// sdf_3.SetFromString(sdf_str_3);
	// auto plugin_sdf_3 = sdf_3.Root()->GetElement("world")->GetElement("plugin");
	// auto node_3 = gazebo_ros::Node::Get(plugin_sdf_3);
	// ASSERT_NE(nullptr, node_3);
	// EXPECT_STREQ("node_3", node_3->get_name());

Fix for multiple video plugins (#898) #937

Fix for multiple video plugins (#898) #937

Conversation

shiveshkhaitan commented Jun 19, 2019

chapulina commented Jun 26, 2019

shiveshkhaitan commented Jun 27, 2019

chapulina Jul 9, 2019

Choose a reason for hiding this comment

shiveshkhaitan Jul 9, 2019

Choose a reason for hiding this comment

shiveshkhaitan Jul 9, 2019 • edited Loading

Choose a reason for hiding this comment

chapulina Jul 9, 2019 • edited Loading

Choose a reason for hiding this comment

chapulina Jul 9, 2019

Choose a reason for hiding this comment

iche033 Jul 9, 2019

Choose a reason for hiding this comment

shiveshkhaitan Jul 10, 2019 • edited Loading

Choose a reason for hiding this comment

chapulina Jul 10, 2019

Choose a reason for hiding this comment

wjwwood Jul 10, 2019

Choose a reason for hiding this comment

chapulina Jul 11, 2019 • edited Loading

Choose a reason for hiding this comment

shiveshkhaitan Jul 9, 2019 •

edited

Loading

chapulina Jul 9, 2019 •

edited

Loading

shiveshkhaitan Jul 10, 2019 •

edited

Loading

chapulina Jul 11, 2019 •

edited

Loading